#include "seu_kernel.h"
#include "seu_external.h"

// the prio rise up
#if ( SEU_THREAD_MAX < 16 )
seu_uint8_t const SEU_THREAD_MAP[16] = {
0u,0u,1u,1u,2u,2u,2u,2u,3u,3u,3u,3u,3u,3u,3u,3u,/*0x00to0x0F*/
};
#else
seu_uint8_t const SEU_THREAD_MAP[256] = {
0u,0u,1u,1u,2u,2u,2u,2u,3u,3u,3u,3u,3u,3u,3u,3u,/*0x00to0x0F*/
4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,4u,/*0x10to0x1F*/
5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,/*0x20to0x2F*/
5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,5u,/*0x30to0x3F*/
6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,/*0x40to0x4F*/
6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,/*0x50to0x5F*/
6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,/*0x60to0x6F*/
6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,6u,/*0x70to0x7F*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0x80to0x8F*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0x90to0x9F*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0xA0to0xAF*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0xB0to0xBF*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0xC0to0xCF*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0xD0to0xDF*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,/*0xE0to0xEF*/
7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u,7u /*0xF0to0xFF*/
};
#endif

struct __SEU_TCB seu_tcb_list[SEU_THREAD_MAX];
struct __SEU_TCB *seu_tcb_cur;
struct __SEU_TCB *seu_tcb_rdy;
struct __SEU_SYS seu_sys;
seu_stk THREAD_IDLE_STK[SEU_IDLE_STK_SIZE];

static void (*thread_idle_hook)(void *param);


static void thread_idle(void)
{
    //if(thread_idle_hook != NULL)
    //thread_idle_hook(NULL);
    while(1)
    {

    }
}

void os_lock(void)
{
    PORT_seu_core_enter();
    seu_sys.lock_nesting++;
    PORT_seu_core_exit();
}

void os_unlock(void)
{
    PORT_seu_core_enter();
    seu_sys.lock_nesting--;
    PORT_seu_core_exit();
}

static void thread_rdy_tbl_set(seu_uint8_t prio)
{
#if (SEU_THREAD_MAX < 16)
    seu_uint8_t grp_prio = prio >> 2;
    seu_sys.thread_rdy_grp_bit |= 1 << grp_prio;
    seu_sys.thread_rdy_tbl_bit[grp_prio] |= (1 << (prio&0x03));
#else
    seu_uint8_t grp_prio = prio >> 3;
    seu_sys.thread_rdy_grp_bit |= 1 << grp_prio;
    seu_sys.thread_rdy_tbl_bit[grp_prio] |= (1 << (prio&0x07));
#endif
}

static void thread_rdy_tbl_reset(seu_uint8_t prio)
{
#if (SEU_THREAD_MAX < 16)
    seu_uint8_t grp_prio = prio >> 2;
    seu_sys.thread_rdy_grp_bit &= ~(1 << grp_prio);
    seu_sys.thread_rdy_tbl_bit[grp_prio] &= ~(1 << (prio&0x03));
#else
    seu_uint8_t grp_prio = prio >> 3;
    seu_sys.thread_rdy_grp_bit &= ~(1 << grp_prio);
    seu_sys.thread_rdy_tbl_bit[grp_prio] &= ~(1 << (prio&0x07));
#endif
}

static seu_rt thread_rdy_tbl_get(seu_uint8_t prio)
{
#if (SEU_THREAD_MAX < 16)
    seu_uint8_t grp_prio = prio >> 2;
    seu_uint8_t gstate = seu_sys.thread_rdy_grp_bit & (1 << grp_prio);
    seu_uint8_t tstate = seu_sys.thread_rdy_tbl_bit[grp_prio] & (1 << (prio & 0x03));
    if(gstate && tstate){
        return SEU_TRUE;
    }
#else
    seu_uint8_t grp_prio = prio >> 3;
    seu_uint8_t gstate = seu_sys.thread_rdy_grp_bit & (1 << grp_prio);
    seu_uint8_t tstate = seu_sys.thread_rdy_tbl_bit[grp_prio] & (1 << (prio & 0x07));
    if(gstate && tstate){
        return SEU_TRUE;
    }
#endif
    return SEU_FALSE;
}

static void thread_cur_num_update(void)
{
    seu_uint8_t grp_prio = SEU_THREAD_MAP[seu_sys.thread_rdy_grp_bit];
#if (SEU_THREAD_MAX < 16)
    seu_sys.thread_cur_num = (grp_prio << 2) + SEU_THREAD_MAP[seu_sys.thread_rdy_tbl_bit[grp_prio]];
#else
    seu_sys.thread_cur_num = (grp_prio << 3) + SEU_THREAD_MAP[seu_sys.thread_rdy_tbl_bit[grp_prio]];
#endif
}

void os_switch(void)
{
    thread_cur_num_update();
    seu_tcb_rdy = &seu_tcb_list[seu_sys.thread_cur_num];
}

void os_sched(void)
{
    if(seu_sys.lock_nesting == 0)
    {
        os_switch();
        if(seu_tcb_rdy != seu_tcb_cur)
        PORT_seu_sched();
    }
}

static seu_stk *seu_stk_init(void *entry, seu_stk *ptopstk)
{
    seu_stk *stk;
    stk = ptopstk;
    stk -= 17;
    *(--stk) = (seu_stk)0x01000000L;
    *(  stk) = (seu_stk)0x01000000L;
    *(--stk) = (seu_stk)entry;
    *(--stk) = (seu_stk)0xFFFFFFFEL;
    stk -= 4;
    *(--stk) = (seu_stk)0;
    stk -= 16;
    stk -= 8;
    return stk;
}

seu_rt seu_thread_create(void *func, seu_stk *ptopstk, seu_uint8_t prio)
{
    if(seu_tcb_list[prio].stk_ptr != (seu_stk*)0){
        if(seu_tcb_list[prio].state != THREAD_STATE_DELETED)
            return SEU_FALSE;
    }
    seu_tcb_list[prio].stk_ptr = seu_stk_init(func, ptopstk);
    seu_tcb_list[prio].prio = prio;
    seu_tcb_list[prio].delay_time = 0;
    seu_tcb_list[prio].state = THREAD_STATE_RUNNING;
    thread_rdy_tbl_set(prio);
    return SEU_TRUE;
}

seu_rt seu_thread_pend(seu_uint8_t prio)
{
		seu_uint8_t ret = SEU_TRUE;
    os_lock();
    seu_tcb_list[prio].state = THREAD_STATE_SUSPEND;
    thread_rdy_tbl_reset(prio);
    os_unlock();
    // only read variable but not write
    os_sched();
		return ret;
}

seu_rt seu_thread_resume(seu_uint8_t prio)
{
    os_lock();
    seu_tcb_list[prio].state = THREAD_STATE_RUNNING;
    thread_rdy_tbl_set(prio);
    os_unlock();
}

seu_rt seu_thread_delete(seu_uint8_t prio)
{
    os_lock();
    seu_tcb_list[prio].state = THREAD_STATE_DELETED;
    // toDO: test if it is [set]
    thread_rdy_tbl_reset(prio);
    os_unlock();
}

void seu_thread_prio_set(seu_uint8_t prio, seu_uint8_t new_prio)
{
    os_lock();
    if(seu_tcb_list[new_prio].state == THREAD_STATE_DELETED){
        seu_tcb_list[new_prio] = seu_tcb_list[prio];
        seu_tcb_list[prio].state = THREAD_STATE_DELETED;
        thread_rdy_tbl_reset(prio);
        if(thread_rdy_tbl_get(prio) == SEU_TRUE)
            thread_rdy_tbl_set(new_prio);
        seu_tcb_list[new_prio].prio = new_prio;
    }
    os_unlock();
    os_sched();
}

void seu_delay(seu_uint32_t tick)
{
    seu_tcb_cur->delay_time = tick;
    seu_sys.thread_slicetime_count = seu_sys.thread_slicetime;
    thread_rdy_tbl_reset(seu_tcb_cur->prio);
    os_sched();
}

void seu_sys_init(void)
{
    seu_uint8_t i = 0;
    for(; i<SEU_THREAD_MAX; i++){
        seu_tcb_list[i].stk_ptr = (seu_stk*)0;
        seu_tcb_list[i].prio = i;
        seu_tcb_list[i].state = THREAD_STATE_DELETED;
        seu_tcb_list[i].delay_time = 0;
        thread_rdy_tbl_reset(i);
    }
    seu_sys.sys_clock_count = 0;
    seu_sys.thread_slicetime = SEU_THREAD_SLICETIME;
    seu_sys.thread_slicetime_count = seu_sys.thread_slicetime;
    seu_sys.sys_print_state = SEU_FALSE;
    seu_sys.os_running = 0;
    seu_thread_create(thread_idle, &THREAD_IDLE_STK[SEU_IDLE_STK_SIZE-1], 0);
    seu_tcb_rdy = &seu_tcb_list[0];
    seu_tcb_cur = &seu_tcb_list[0];

}

void seu_sys_start(void)
{
    PORT_seu_systick_init();
    if(_seu_user_init != NULL){
        _seu_user_init();
    }
    PORT_seu_systick_init();
    PORT_seu_systick_irq_ctrl(SEU_TRUE);
    PORT_seu_psp_reset();
    PORT_seu_svc_init();
    seu_sys.os_running = 1;
    PORT_seu_sched();
    while(1);
}

void seu_systick_handler(void)
{
    if (!seu_sys.os_running) {
        return NULL;
    }
    seu_uint8_t idx = 0;
    seu_sys.sys_clock_count++;
    for(; idx<SEU_THREAD_MAX; idx++){
        if(seu_tcb_list[idx].state == THREAD_STATE_RUNNING){
            // how to acertain Atomicity of instructions
            if((seu_tcb_list[idx].delay_time--) <= 0){
                thread_rdy_tbl_set(seu_tcb_list[idx].prio);
                seu_tcb_list[idx].delay_time = 0;
            }
        }
    }
#if (SEU_TIMER_MAX > 0)
    for(idx=0; idx<SEU_TIMER_MAX; idx++){
        if((seu_timer[idx].statue == SEU_TRUE) && ((seu_timer[idx].timer_count--) <= 0)){
            if(seu_timer[idx].timer_flag == TIMER_ONECE){
                seu_timer[idx].statue = SEU_FALSE;
                seu_timer[idx].timer_count = 0;
            }else{
                seu_timer[idx].timer_count = seu_timer[idx].timer_reload;
            }
            if(seu_timer[idx].timer_handler != NULL){
                seu_timer[idx].timer_handler(seu_timer[idx].timer_param.vect, seu_timer[idx].timer_param.config);
            }
        }
    }
#endif
    if((seu_sys.thread_slicetime_count--) == 0){
        seu_sys.thread_slicetime_count = seu_sys.thread_slicetime;
        os_sched();
    }
}
